//
// Created by liangwenhao on 2024/4/4.
//

#include "motor_driver.h"
#include "string.h"
#include "math.h"
#include "bsp_can.h"
/**
  * @brief  Motor Init
  * @param  * motor motor
  * @param  num Motor IDs in CAN communication
  * @param  motor_form Categories based on motor functionality
  * @param  formID  Numbering of motors in different categories
  * @retval None
  */
void Motor_Init(Motor * motor,uint8_t num,uint8_t motor_form,uint8_t formID)
{
    memset(motor, 0, sizeof(*motor));///将结构体成员值归零
    motor->Motor_Data.Number=num;
    motor->Flag.Angle_begin_record = 0;
    motor->Flag.Shaking = 0;
    switch(motor_form)
    {
        case YawMotorForm:
        {
            motor->Motor_Data.StdID=0x204;
            motor->Motor_Data.torque_current_max=14000;
            motor->Flag.Reset=1;
            motor->PID_Speed.Kp=8; //5
            motor->PID_Speed.Ki=0;
            motor->PID_Speed.Kd=0;
            motor->PID_Angle.Kp=0.4; //-2
            motor->PID_Angle.Ki=0;
            motor->PID_Angle.Kd=0;
            break;
        }
        case LoadMotorForm:
        {
            if(formID==1)
            {
                motor->Motor_Data.StdID=0x201;
                motor->Motor_Data.torque_current_max=14000;//6000
                motor->Flag.Reset=1;
                motor->PID_Speed.Kp=8;
                motor->PID_Speed.Ki=0;
                motor->PID_Speed.Kd=0;
                motor->PID_Angle.Kp=0.4f;
                motor->PID_Angle.Ki=0;
                motor->PID_Angle.Kd=0;
            }
            else if(formID==2)
            {
                motor->Motor_Data.StdID=0x203;
                motor->Motor_Data.torque_current_max=14000;//6000
                motor->Flag.Reset=1;
                motor->PID_Speed.Kp=8;
                motor->PID_Speed.Ki=0;
                motor->PID_Speed.Kd=0;
                motor->PID_Angle.Kp=0.4f;
                motor->PID_Angle.Ki=0;
                motor->PID_Angle.Kd=0;
            }
            else if(formID==3)
            {
                motor->Motor_Data.StdID=0x205;
                motor->Motor_Data.Type=DJI_M2006;
                motor->Flag.Reset=1;
                motor->PID_Speed.Kp=0;
                motor->PID_Speed.Ki=0;
                motor->PID_Speed.Kd=0;
                motor->PID_Angle.Kp=0;
                motor->PID_Angle.Ki=0;
                motor->PID_Angle.Kd=0;
            }
            else if(formID==4)
            {
                motor->Motor_Data.StdID=0x205;
                motor->Motor_Data.Type=DJI_M2006;
                motor->Flag.Reset=1;
                motor->PID_Speed.Kp=0;
                motor->PID_Speed.Ki=0;
                motor->PID_Speed.Kd=0;
                motor->PID_Angle.Kp=0;
                motor->PID_Angle.Ki=0;
                motor->PID_Angle.Kd=0;
            }
            else if(formID==5)
            {
                motor->Motor_Data.StdID=0x205;
                motor->Motor_Data.Type=DJI_M2006;
                motor->Flag.Reset=1;
                motor->PID_Speed.Kp=0;
                motor->PID_Speed.Ki=0;
                motor->PID_Speed.Kd=0;
                motor->PID_Angle.Kp=0;
                motor->PID_Angle.Ki=0;
                motor->PID_Angle.Kd=0;
            }
            break;
        }
        case LocateMotorForm:
        {
            motor->Motor_Data.StdID=0x202;
            motor->Motor_Data.torque_current_max=14000;
            motor->Flag.Reset=1;
            motor->PID_Speed.Kp=8;
            motor->PID_Speed.Ki=0;
            motor->PID_Speed.Kd=0;
            motor->PID_Angle.Kp=0.4f;
            motor->PID_Angle.Ki=0.00015f;
            motor->PID_Angle.Kd=0;
            break;
        }
        default:
            break;
    }

}

void Motor_canTx_calculation(Motor * motor,CAN_Send * canTx)///canTx是数组，不是结构体
{
    uint8_t num = motor->Motor_Data.Number;
    uint8_t index = (num < 5 ? num : num - 4) * 2 - 1;
    canTx->SendData[index - 1] = (motor->Motor_Data.current & 0xFF00) >> 8;
    canTx->SendData[index] = (motor->Motor_Data.current & 0x00FF);
}

void Motor_canRx_calculation(Motor * motor,const uint8_t * canRx)
{
    motor->Motor_Data.real_angle_last=motor->Motor_Data.real_angle;
    motor->PID_Angle.round_num_last=motor->PID_Angle.round_num;
    motor->Motor_Data.real_speed=(int16_t)(canRx[2]<<8|canRx[3]);
    motor->Motor_Data.real_angle=(int16_t)(canRx[0]<<8|canRx[1]);
    motor->Motor_Data.torque_current=(int16_t)(canRx[4]<<8|canRx[5]);

    motor->Motor_Data.real_speed=motor->Motor_Data.real_speed*0.9f+motor->Motor_Data.real_speed_last*0.1f;
    motor->Motor_Data.real_speed_last=motor->Motor_Data.real_speed;
    // 设定速度

    if(fabsf(motor->Motor_Data.torque_current)-fabsf(motor->Motor_Data.torque_current_max)>=0&&motor->Motor_Data.real_speed<50)
    {
        motor->Flag.over_num++;

    }
    if(motor->Flag.over_num>=20&&motor->Motor_Data.real_speed<50)
    {
        motor->Flag.Stalled=1;
    }

    if(motor->Flag.Reset==1)//判断速度环或位置环
    {
        //记圈，使用归一后的数据圈数会跳变
        if(motor->Motor_Data.real_angle_last-motor->Motor_Data.real_angle>4096)
        {
            motor->PID_Angle.round_num++;
        }
        else if(motor->Motor_Data.real_angle_last-motor->Motor_Data.real_angle<-4096)
        {
            motor->PID_Angle.round_num--;
        }
    }

    if(motor->Motor_Data.Type==DJI_M3508)//Yaw轴电机不用这个解算函数
    {
        motor->PID_Speed.Real=(float)(motor->Motor_Data.real_speed/MOTOR3508_SPEED_MAX);
        motor->PID_Angle.Real=(float)(motor->Motor_Data.real_angle/MOTOR3508_ANGLE_MAX+motor->PID_Angle.round_num);
    }
    else if(motor->Motor_Data.Type==DJI_M2006)
    {
        motor->PID_Speed.Real=(float)(motor->Motor_Data.real_speed/MOTOR2006_SPEED_MAX);
        motor->PID_Angle.Real=(float)(motor->Motor_Data.real_angle/MOTOR2006_ANGLE_MAX+motor->PID_Angle.round_num);
    }
    else if(motor->Motor_Data.Type==DJI_GM6020)
    {
        motor->PID_Speed.Real=(float)(motor->Motor_Data.real_speed/MOTOR6020_SPEED_MAX);
        motor->PID_Angle.Real=(float)(motor->Motor_Data.real_angle/MOTOR6020_ANGLE_MAX+motor->PID_Angle.round_num);
    }

    if(motor->Flag.Reset==1)//判断速度环或位置环
    {
        if (motor->Flag.Angle_begin_record == 0) {
            motor->PID_Angle.Angle_begin = motor->PID_Angle.Real;
            motor->PID_Angle.Target = motor->PID_Angle.Real;
            motor->Flag.Angle_begin_record++;
        }
    }
}


void Motor_Flag_change(Motor * motor)
{
//    if(motor->Flag.Stalled==1)
//    {
//        osDelay(1000);
//        motor->Flag.Stalled=0;
//    }
    if(motor->Flag.over_num>0)
    {
        osDelay(1500);
        motor->Flag.over_num--;
    }
}

///这里的设定速度是实际速度
void Motor_real_speed_set(Motor * motor,float real_speed,CAN_Send  *can_send)
{
    motor->Motor_Data.target_speed=real_speed;
    if(motor->Motor_Data.Type==DJI_M3508)
    {
        motor->PID_Speed.Target=(float)real_speed/MOTOR3508_SPEED_MAX;
        PIDCalculation_Speed(motor);
        motor->Motor_Data.current=(int16_t)(motor->PID_Speed.Out*MOTOR3508_CURRENT_MAX);
    }
    else if(motor->Motor_Data.Type==DJI_M2006)
    {
        motor->PID_Speed.Target=(float)real_speed/MOTOR2006_SPEED_MAX;
        PIDCalculation_Speed(motor);
        motor->Motor_Data.current=(int16_t)(motor->PID_Speed.Out*MOTOR2006_CURRENT_MAX);
    }
    else if(motor->Motor_Data.Type==DJI_GM6020)
    {
        motor->PID_Speed.Target=(float)real_speed/MOTOR6020_SPEED_MAX;
        PIDCalculation_Speed(motor);
        motor->Motor_Data.current=(int16_t)(motor->PID_Speed.Out*MOTOR6020_VOLTAGE_MAX);
    }
    if(motor->Flag.Stalled==1)
    {
        motor->Motor_Data.current=0;
    }
    Motor_canTx_calculation(motor,can_send);
}

///设定角度为实际角度
void Motor_real_angle_set(Motor * motor,float real_angle,CAN_Send * can_send)
{
    motor->PID_Angle.Cross=(float)real_angle/8191;
    motor->PID_Angle.Target=(float)(motor->PID_Angle.Angle_begin+motor->PID_Angle.Cross);
    PIDCalculation_Angle(motor);
    motor->PID_Speed.Target=motor->PID_Angle.Out;
    if(motor->PID_Speed.Target>1)
    {
        motor->PID_Speed.Target=1;
    }
    else if(motor->PID_Speed.Target<-1)
    {
        motor->PID_Speed.Target=-1;
    }
    PIDCalculation_Speed(motor);
    if(motor->Motor_Data.Type==DJI_M3508)
    {
        motor->Motor_Data.target_speed=motor->PID_Speed.Target*MOTOR3508_SPEED_MAX;
        motor->Motor_Data.current=(int16_t)(motor->PID_Speed.Out*MOTOR3508_CURRENT_MAX);
    }
    else if(motor->Motor_Data.Type==DJI_M2006)
    {
        motor->Motor_Data.target_speed=motor->PID_Speed.Target*MOTOR2006_SPEED_MAX;
        motor->Motor_Data.current=(int16_t)(motor->PID_Speed.Out*MOTOR2006_CURRENT_MAX);
    }
    else if(motor->Motor_Data.Type==DJI_GM6020)
    {
        motor->Motor_Data.target_speed=motor->PID_Speed.Target*MOTOR6020_SPEED_MAX;
        motor->Motor_Data.current=(int16_t)(motor->PID_Speed.Out*MOTOR6020_VOLTAGE_MAX);
    }
    if(motor->Flag.Stalled==1)
    {
        motor->Motor_Data.current=0;
    }
    Motor_canTx_calculation(motor,can_send);
}


///电机复位函数
void Motor_reset_set(Motor *motor,CAN_Send canSend,float reset_speed,float increment_angle)
{
    if(motor->Flag.Stalled==1)
    {
        motor->Flag.Reset=1;
    }
    if(motor->Flag.Reset==0)
    {
        Motor_real_speed_set(motor,reset_speed,&canSend);
    }
    else if(motor->Flag.Reset==1)
    {
        motor->Flag.Stalled=0;
        motor->PID_Angle.Cross+=(float)increment_angle/8191;
        Motor_real_angle_set(motor,motor->PID_Angle.Cross,&canSend);
    }
}

float YawMotor_MoveAngle;
void YawMotor_angle_set(float TargetAngle)
{
    if(RC_Ctl.Rc_Dog.rc_dog<1)
    {
        YawMotor_MoveAngle = (float)(TargetAngle - YawEncoder_Real_Angle)*600000;
        if(YawMotor_MoveAngle > 800) YawMotor_MoveAngle = 800;
        if(YawMotor_MoveAngle < -800) YawMotor_MoveAngle = -800;
        if(fabsf(YawMotor_MoveAngle) < 15) YawMotor_MoveAngle = 0;
        YawMotor_CrossAngle -= YawMotor_MoveAngle;
    }
    Motor_real_angle_set(&YawMotor,YawMotor_CrossAngle,&can1Tx_0x200);
}


float Angle_Error1;
void LoadMotor_real_angle_set(float angle_cross1)
{
    Angle_Error1 = LoadMotor1.PID_Angle.Cross + LoadMotor2.PID_Angle.Cross;



    LoadMotor1.PID_Angle.Cross=(float)angle_cross1/8191;
    LoadMotor1.PID_Angle.Target=(float)(LoadMotor1.PID_Angle.Angle_begin+LoadMotor1.PID_Angle.Cross);
    PIDCalculation_Angle(&LoadMotor1);
    LoadMotor1.PID_Speed.Target = LoadMotor1.PID_Angle.Out - 0.5 * Angle_Error1;
    if(LoadMotor1.PID_Speed.Target>1)
    {
        LoadMotor1.PID_Speed.Target=1;
    }
    else if(LoadMotor1.PID_Speed.Target<-1)
    {
        LoadMotor1.PID_Speed.Target=-1;
    }
    PIDCalculation_Speed(&LoadMotor1);

    LoadMotor1.Motor_Data.target_speed=LoadMotor1.PID_Speed.Target*MOTOR3508_SPEED_MAX;
    LoadMotor1.Motor_Data.current=(int16_t)(LoadMotor1.PID_Speed.Out*MOTOR3508_CURRENT_MAX);

    if(LoadMotor1.Flag.Stalled==1)
    {
        LoadMotor1.Motor_Data.current=0;
    }

    Motor_canTx_calculation(&LoadMotor1,&can1Tx_0x200);



    LoadMotor2.PID_Angle.Cross=(float)((-1)*angle_cross1)/8191;
    LoadMotor2.PID_Angle.Target=(float)(LoadMotor2.PID_Angle.Angle_begin+LoadMotor2.PID_Angle.Cross);
    PIDCalculation_Angle(&LoadMotor2);
    LoadMotor2.PID_Speed.Target=LoadMotor2.PID_Angle.Out - 0.5 * Angle_Error1;
    if(LoadMotor2.PID_Speed.Target>1)
    {
        LoadMotor2.PID_Speed.Target=1;
    }
    else if(LoadMotor2.PID_Speed.Target<-1)
    {
        LoadMotor2.PID_Speed.Target=-1;
    }
    PIDCalculation_Speed(&LoadMotor2);

    LoadMotor2.Motor_Data.target_speed=LoadMotor2.PID_Speed.Target*MOTOR3508_SPEED_MAX;
    LoadMotor2.Motor_Data.current=(int16_t)(LoadMotor2.PID_Speed.Out*MOTOR3508_CURRENT_MAX);

    if(LoadMotor2.Flag.Stalled==1)
    {
        LoadMotor2.Motor_Data.current=0;
    }
    Motor_canTx_calculation(&LoadMotor2,&can1Tx_0x200);
}
